/*
 * DriveTrain.java
 * 
 */

package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.TimerTask;

/**
 *
 * @author Driver
 */
public class DriveTrain extends TimerTask 
{
    private Victor sweeperMtr;
    private Victor conveyorMtr;
    
    private Victor rrDriveMtr;
    private Victor lfDriveMtr;
    //for the additional motors:
    private Victor lrDriveMtr;
    private Victor rfDriveMtr;
    
    private Victor lrRfSteerMtr;
    private Victor lfRrSteerMtr;

    private Joystick driverJoy;

    private PIDController lrRfSteerCtrl;
    private PIDController lfRrSteerCtrl;
    private MagneticEncoder lfRrSteerEnc;
    private MagneticEncoder lrRfSteerEnc;
    
    public double oldangle = 0;
    public double newangle = 0;
    public double target = 0;
    public double current = 0;
    public SmartDashboard myDash = new SmartDashboard();
    private boolean TWIST = true;
    private boolean STRAIGHT = false;

    public DriveTrain()
    {
        sweeperMtr = new Victor(Constants.SWEEPERMTR_SLOT);
        conveyorMtr = new Victor(Constants.CONVEYORMTR_SLOT);
        
        //motors added with the new Drive Train:
        rrDriveMtr = new Victor(Constants.RR_DRIVE_SLOT);
        lfDriveMtr = new Victor(Constants.LF_DRIVE_SLOT);
        rfDriveMtr = new Victor(Constants.RF_DRIVE_SLOT); // be ready to disable
        lrDriveMtr = new Victor(Constants.LR_DRIVE_SLOT); // be ready to disable
       
        // Motor for Steering
        lrRfSteerMtr = new Victor(Constants.LR_RF_STEERMTR_SLOT);
        lfRrSteerMtr = new Victor(Constants.LF_RR_STEERMTR_SLOT);//be ready to disable
        
        driverJoy = new Joystick(Constants.DRIVER_JOY);
        
        // Steering Encoders
        lfRrSteerEnc = new MagneticEncoder(Constants.CRIO_ANALOG,
                Constants.LF_RR_STEER_ENC);
        lrRfSteerEnc = new MagneticEncoder(Constants.CRIO_ANALOG,
                Constants.LR_RF_STEER_ENC);
        // Steering Controllers
        lrRfSteerCtrl = createSteerPID(lrRfSteerEnc,lrRfSteerMtr);
        lfRrSteerCtrl = createSteerPID(lfRrSteerEnc,lfRrSteerMtr);
        
        lrRfSteerCtrl.setPID(Constants.P_STEER_CONST,
                Constants.I_STEER_CONST,Constants.D_STEER_CONST);
        lfRrSteerCtrl.setPID(Constants.P_STEER_CONST,
                Constants.I_STEER_CONST,Constants.D_STEER_CONST);
    }

    public void run() {
        
        sweeperControl();
        
        /*
          * Grabs the angle from the X and Y vectors of the joystick,
        */ 
        final double X_DEAD = 0.3;
        final double Y_DEAD = 0.1;
        double y = driverJoy.getY()*-1; //invert Y axis
        double x = driverJoy.getX();

       if ( goStraight())
       {
            straightDrive(Utils.getJoyMagnitude(x, y));
       }
       else if(goTwist())
       {  
            twistDrive();
       }
       else
       {          
            /*
             * Find dead zone
             */
            x = (Math.abs(x)<X_DEAD) ? 0 : x;
            y = (Math.abs(y)<Y_DEAD) ? 0 : y;
                        
            if(x != 0 || y != 0)
            {
                normalDrive(x,y);   // in action zone
            }
            else
            {
                noDrive();      // in dead zone
            }
       }
        /*
         * Dashboard output
         */
        
        myDash.putString("X",String.valueOf(Utils.fmt6_3(x)));
        myDash.putString("Y",String.valueOf(Utils.fmt6_3(y)));
        myDash.putString("EncoderLFRR",String.valueOf(Utils.fmt6_3(lfRrSteerEnc.pidGet())));
        myDash.putString("SetpointLFRR",String.valueOf(Utils.fmt6_3(lfRrSteerCtrl.getSetpoint())));
        myDash.putString("PID OutputLFRR",String.valueOf(Utils.fmt6_3(lfRrSteerCtrl.get())));
        myDash.putString("EncoderLRRF",String.valueOf(Utils.fmt6_3(lrRfSteerEnc.pidGet())));
        myDash.putString("SetpointLRRF",String.valueOf(Utils.fmt6_3(lrRfSteerCtrl.getSetpoint())));
        myDash.putString("PID OutputLRRF",String.valueOf(Utils.fmt6_3(lrRfSteerCtrl.get())));
        myDash.putString("P",String.valueOf(lrRfSteerCtrl.getP()));
        myDash.putString("I",String.valueOf(lrRfSteerCtrl.getI()));
        myDash.putString("D",String.valueOf(lrRfSteerCtrl.getD()));
        myDash.putString("Current Button",String.valueOf(Utils.getLowestButton(driverJoy)));
    }    
    /*
     * Puts robot into twist mode
     */
    public void twistDrive()
    {
        /*
         * Change PID constants to their Twist Drive versions
         */
        // lrRfSteerCtrl.setPID(Constants.P_TWIST_CONST,Constants.I_TWIST_CONST,Constants.D_TWIST_CONST);
       //lfRrSteerCtrl.setPID(Constants.P_TWIST_CONST,Constants.I_TWIST_CONST,Constants.D_TWIST_CONST);
 
         double twistSpeed = driverJoy.getTwist();
         setSteer(Constants.TWIST_VALUE);
         setTwistDrive(twistSpeed);
         myDash.putString("TwistValue",String.valueOf(Utils.fmt6_3(twistSpeed)));
         myDash.putString("DriveMode","Twist");
    }
    
    private void normalDrive( double x, double y)
    {
        /*
         * Change PID constants to their Normal Drive versions
         */
//        lrRfSteerCtrl.setPID(Constants.P_STEER_CONST,
//                Constants.I_STEER_CONST,Constants.D_STEER_CONST);
//        lfRrSteerCtrl.setPID(Constants.P_STEER_CONST,
//                Constants.I_STEER_CONST,Constants.D_STEER_CONST);
        /* 
         * Retrieve and normalize angle
         */
        double angle = getAngle(x,y);
        angle = ((angle)/360.0);  
        /*
         * Retrieve joystick magnitude
         */
        double joyMag = Utils.getJoyMagnitude(x, y);
        /* 
         * Apply angle and magnitude to appropriate motors
         */
        setSteer(angle);
        setNormalDrive(joyMag);
        myDash.putString("TwistValue","");
        myDash.putString("DriveMode","Normal");
    }
    
    private void straightDrive(double speed)
    {
         setSteer(Constants.STRAIGHT_POSITION);
         setNormalDrive(speed);
         myDash.putString("TwistValue","");
         myDash.putString("DriveMode","Straight");
    }
    
    private void noDrive()
    {
       //  setSteer(Constants.STRAIGHT_POSITION);
         setNormalDrive(0); // turn off drive motors
         myDash.putString("TwistValue","");
         myDash.putString("DriveMode","Dead Zone");
    }
    
      private void setSteer( double angle)
    {
         double offset = lfRrSteerCtrl.get() - lrRfSteerCtrl.get();
         lfRrSteerCtrl.setSetpoint(angle);
         lrRfSteerCtrl.setSetpoint(angle);
         myDash.putString("Angle",Utils.fmt6_3(angle));
    }
    
    private void setNormalDrive(double speed)
    {
        setSpeed(speed, STRAIGHT);
    }
    /*
     * used to invert directions of wheels so that robot can rotate properly
     */ 
    private void setTwistDrive(double speed)
    {
        setSpeed(speed, TWIST);
    }
    
    private void setSpeed( double speed, boolean twist)
    {
         // CHECK THIS !!!!!!!!
         //rrDriveMtr.set(twist? -speed : speed);
         //lfDriveMtr.set(twist? -speed : speed);
         //lrDriveMtr.set(speed);
         //rfDriveMtr.set(speed);
         myDash.putString("Speed",String.valueOf(Utils.fmt6_3(speed)));
    }
    
    

    public double getAngle(double x, double y){
        double angle = Utils.calculateAng(x, y);
       
        angle = 90-angle;
        /*
         * Adapt angle so that the straight value is
         * consistent with the Constants.STRAIGHT_POSITION constant
         */
        angle += Constants.STRAIGHT_POSITION*360;
        angle = Utils.mod360( angle );
        //angle = getRotaryShortestPath(angle, oldangle);
        oldangle = angle;
        return angle;
    }

    /**
     * RotaryShortestPath()
     *
     * Returns the shortest delta path from current angle to target angle.
     *
     */
//    private double getRotaryShortestPath(double target, double current )
//-    {
//        //System.out.println("DriveTrain::getRotaryShortestPath");
//        double output = target - current;
//
//        output = output % 360.0;
//
//        if( output < -180.0 )
//            output = 360.0 + output;
//        if( output > 180.0 )
//            output = output - 360.0;
//
//        return output;
//    }
    
    private PIDController createSteerPID( PIDSource source, PIDOutput output )
    {
       PIDController ctlr = new PIDController(Constants.P_STEER_CONST, Constants.I_STEER_CONST, Constants.D_STEER_CONST, source, output);
       ctlr.setInputRange(0, 1.0);
       ctlr.setOutputRange(-1.0, 1.0);
       //ctlr.setOutputRange(-0.5, 0.5);
       ctlr.setTolerance(1.0);

       ctlr.setContinuous();
       ctlr.enable();
       System.out.print("created steering PID");
       return ctlr;
    }
    
    private boolean goStraight()
    {
        return driverJoy.getRawButton(Constants.STRAIGHT_POSITION_BTN);      
    }
    
    private boolean goTwist()
    {
        return driverJoy.getRawButton(Constants.TWIST_POSITION_BTN);
    }

    public void reset() 
    {
         lfRrSteerCtrl.reset();
         lrRfSteerCtrl.reset();
         lfRrSteerCtrl.enable();
         lrRfSteerCtrl.enable();
    }

    /**
     * Sweeper/Conveyor works independent of Shooter
     */
    private void sweeperControl()
    {
        boolean sweeperOnOrOff = driverJoy.getRawButton(Constants.SWEEPER_BUTTON);
        //activate the sweeper motor.
        sweeperMtr.set(sweeperOnOrOff ? 1.0 : 0.0);
        conveyorMtr.set(sweeperOnOrOff ? 1.0 : 0.0);
    }
    
 
}
